﻿#include <gtest/gtest.h>

#include "kernel/kernel.h"
#include "kernel/kerstr.h"
#include "kernel/transform.h"
#include "math/linear_math.h"

#include <algorithm>
#include <iostream>

#include <dbg.h>

using namespace CAGD;

using BXYZ = TCoordN<3, double, CoordsType::Vec>;
using BXY = TCoordN<2, double, CoordsType::Vec>;

// 测试基本内联函数
TEST(TestInline, BasisFunctions)
{
    EXPECT_EQ(120, factorial(5));
    EXPECT_FLOAT_EQ(10, combine(5, 3));
    EXPECT_FLOAT_EQ(20, permutation(5, 3));
    EXPECT_FLOAT_EQ(0.263671875, bernstein(5, 3, 0.25));
    EXPECT_EQ(combine(BPnt(1, 2, 3), BPnt(4, 5, 6), 1.5, 2.5), BPnt(2.875, 3.875, 4.875));

    EXPECT_EQ(homogenous_to_cartesian(BPnt4d(1, 2, 3, 2.0)), BPnt(0.5, 1, 1.5));
    EXPECT_EQ(homogenous_to_cartesian(BVec4d(1, 2, 3, 0)), BVec(1, 2, 3));
    EXPECT_EQ(cartesian_to_homogenous(BPnt(0.5, 1, 1.5)), BPnt4d(0.5, 1, 1.5, 1));
    EXPECT_EQ(cartesian_to_homogenous(BVec(0.5, 1, 1.5)), BVec4d(0.5, 1, 1.5, 0));

    EXPECT_TRUE(on_line(BPnt(1, 2, 3), BPnt(1, 2, 3), BPnt(1, 2, 3)));
    EXPECT_TRUE(on_line(BPnt(1, 2, 3), BPnt(1, 2, 3), BPnt(4, 5, 6)));
    EXPECT_TRUE(on_line(BPnt(1, 2, 3), BPnt(4, 5, 6), BPnt(4, 5, 6)));
    EXPECT_FALSE(on_line(BPnt(1, 2, 3), BPnt(4, 6, 6), BPnt(4, 5, 6)));

    EXPECT_TRUE(on_face(BPnt(1, 2, 3), BPnt(1, 2, 3), BPnt(1, 2, 3), BPnt(1, 2, 3)));
    EXPECT_TRUE(on_face(BPnt(1, 2, 3), BPnt(1, 2, 3), BPnt(1, 2, 3), BPnt(4, 5, 6)));
    EXPECT_TRUE(on_face(BPnt(1, 2, 3), BPnt(1, 2, 3), BPnt(4, 5, 6), BPnt(4, 5, 6)));
    EXPECT_TRUE(on_face(BPnt(1, 2, 3), BPnt(4, 5, 6), BPnt(4, 5, 6), BPnt(4, 5, 6)));
    EXPECT_TRUE(on_face(BPnt(1, 2, 3), BPnt(4, 5, 6), BPnt(7, 8, 9), BPnt(2, 8, 6)));

    EXPECT_FALSE(on_face(BPnt(1, 2, 3), BPnt(2, 5, 3), BPnt(1, 9, 3), BPnt(2, 8, 6)));
}

// 测试 BXYZ
TEST(ATestBXYZ, BasisFunctions)
{
    BXYZ P = {1.2, 3.4, 5.6};
    EXPECT_FLOAT_EQ(P.X(), 1.2);
    EXPECT_FLOAT_EQ(P.Y(), 3.4);
    EXPECT_FLOAT_EQ(P.Z(), 5.6);
    EXPECT_TRUE(P == BXYZ(1.2, 3.4, 5.6));
    BXYZ Q(1.2, 3.4, 5.6);

    BXYZ P2 = {1.1, 3.3, 5.5};
    EXPECT_TRUE((P + P2) == BXYZ({2.3, 6.7, 11.1}));
    EXPECT_TRUE((P - P2) == BXYZ(0.1, 0.1, 0.1));
    EXPECT_TRUE((P * 2) == BXYZ(2.4, 6.8, 11.2));
    EXPECT_TRUE((P / 2) == BXYZ(0.6, 1.7, 2.8));

    BXY PXY = {1.2, 3.4};
    BXYZ PXYZ = PXY;
    EXPECT_TRUE(PXYZ == BXYZ({1.2, 3.4, 0.0}));

    P = {1, 2, 3};
    P2 = {4, 5, 6};
    EXPECT_FLOAT_EQ(P * P2, 32);
    EXPECT_TRUE((P % P2) == BXYZ(-3, 6, -3));
    EXPECT_FLOAT_EQ(P.Modulus(), sqrt(14));
    EXPECT_TRUE(P.Normalized() == (BXYZ(1, 2, 3) / sqrt(14)));

    P.Normalize();
    EXPECT_TRUE(P == (BXYZ(1, 2, 3) / sqrt(14)));

    P.SetX(5);
    P.SetY(6);
    P.SetZ(7);
    EXPECT_TRUE(P == BXYZ(5, 6, 7));

    P += P2;
    EXPECT_TRUE(P == BXYZ(9, 11, 13));

    P -= P2;
    EXPECT_TRUE(P == BXYZ(5, 6, 7));

    P = {1, 2, 3};
    P2 = {4, 5, 6};
    P %= P2;
    EXPECT_TRUE(P == BXYZ(-3, 6, -3));

    P *= 2;
    EXPECT_TRUE(P == BXYZ(-6, 12, -6));

    P /= 2;
    EXPECT_TRUE(P == BXYZ(-3, 6, -3));

    const double *data = P.GetData();
    EXPECT_FLOAT_EQ(data[0], -3);
    EXPECT_FLOAT_EQ(data[1], 6);
    EXPECT_FLOAT_EQ(data[2], -3);

    double *data2 = P.ChangeData();
    data2[0] = 1;
    data2[1] = 2;
    data2[2] = 3;
    EXPECT_TRUE(P == BXYZ(1, 2, 3));
}

// 测试 BPnt
TEST(ATestBPnt, BasisFunctions)
{
    BPnt P = {1.2, 3.4, 5.6};
    EXPECT_FLOAT_EQ(P.X(), 1.2);
    EXPECT_FLOAT_EQ(P.Y(), 3.4);
    EXPECT_FLOAT_EQ(P.Z(), 5.6);
    EXPECT_TRUE(P == BPnt(1.2, 3.4, 5.6));

    BVec V2 = {1.1, 3.3, 5.5};
    EXPECT_TRUE((P + V2) == BPnt(2.3, 6.7, 11.1));
    EXPECT_TRUE((P - V2) == BPnt(0.1, 0.1, 0.1));

    BPnt P3 = {1.1, 3.3, 5.5};
    EXPECT_TRUE((P - P3) == BVec(0.1, 0.1, 0.1));
    EXPECT_FLOAT_EQ(P.Distance(P3), 0.1 * sqrt(3));

    P.SetX(5);
    P.SetY(6);
    P.SetZ(7);
    EXPECT_TRUE(P == BPnt(5, 6, 7));

    P = {1.2, 3.4, 5.6};
    P3 = {1.1, 3.3, 5.5};
    EXPECT_TRUE(combine(P, P3, 0.5, 2.0) == BPnt(BXYZ(2.8, 8.3, 13.8) / 2.5));

    P = {1, 2, 3};
    BPnt P2 = {2, 3, 4};
    P3 = {3, 4, 5};
    BPnt P4 = {4, 4, 1};
    BPnt P5 = {4, 2, 1};
    EXPECT_TRUE(on_line(P, P2, P3));
    EXPECT_FALSE(on_line(P, P2, P4));
    EXPECT_TRUE(on_face(P, P2, P3, P4));
    EXPECT_FALSE(on_face(P, P2, P4, P5));
}

// 测试 BXY
TEST(ATestBXY, BasisFunctions)
{
    BXY P = {1.2, 3.4};
    EXPECT_FLOAT_EQ(P.X(), 1.2);
    EXPECT_FLOAT_EQ(P.Y(), 3.4);
    EXPECT_TRUE(P == BXY(1.2, 3.4));

    BXY P2 = {1.1, 3.3};
    EXPECT_TRUE((P + P2) == BXY(2.3, 6.7));
    EXPECT_TRUE((P - P2) == BXY(0.1, 0.1));
    EXPECT_TRUE((P * 2) == BXY(2.4, 6.8));
    EXPECT_TRUE((P / 2) == BXY(0.6, 1.7));

    P = {1, 2};
    P2 = {4, 5};
    EXPECT_FLOAT_EQ(P * P2, 14);
    EXPECT_FLOAT_EQ(P.Modulus(), sqrt(5));
    EXPECT_TRUE(P.Normalized() == (BXY(1, 2) / sqrt(5)));

    P.Normalize();
    EXPECT_TRUE(P == (BXY(1, 2) / sqrt(5)));

    P.SetX(5);
    P.SetY(6);
    EXPECT_TRUE(P == BXY(5, 6));

    P += P2;
    EXPECT_TRUE(P == BXY(9, 11));

    P -= P2;
    EXPECT_TRUE(P == BXY(5, 6));

    P *= 2;
    EXPECT_TRUE(P == BXY(10, 12));

    P /= 2;
    EXPECT_TRUE(P == BXY(5, 6));

    const double *data = P.GetData();
    EXPECT_FLOAT_EQ(data[0], 5);
    EXPECT_FLOAT_EQ(data[1], 6);

    double *data2 = P.ChangeData();
    data2[0] = 1;
    data2[1] = 2;
    EXPECT_TRUE(P == BXY(1, 2));
}

// 测试 BPnt2d
TEST(ATestBPnt2d, BasisFunctions)
{
    BPnt2d P = {1.2, 3.4};
    EXPECT_FLOAT_EQ(P.X(), 1.2);
    EXPECT_FLOAT_EQ(P.Y(), 3.4);
    EXPECT_TRUE(P == BPnt2d(1.2, 3.4));

    BVec2d P2 = {1.1, 3.3};
    EXPECT_TRUE((P + P2) == BPnt2d(2.3, 6.7));
    EXPECT_TRUE((P - P2) == BPnt2d(0.1, 0.1));

    BPnt2d P3 = {1.1, 3.3};
    EXPECT_TRUE((P - P3) == BVec2d(0.1, 0.1));
    EXPECT_FLOAT_EQ(P.Distance(P3), 0.1 * sqrt(2));

    P.SetX(5);
    P.SetY(6);
    EXPECT_TRUE(P == BPnt2d(5, 6));

    P = {1.2, 3.4};
    P3 = {1.1, 3.3};
    EXPECT_TRUE(combine(P, P3, 0.5, 2.0) == BPnt2d(BXY(2.8, 8.3) / 2.5));
}

// 测试矩阵
TEST(TestMat, BasisFunctions)
{
    // 测试比较
    {
        EXPECT_TRUE(BMat3x4({1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                    BMat3x4({1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));
        EXPECT_TRUE(BMat4x3({1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                    BMat4x3({1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));
    }

    // 测试初始化
    {
        BMat4x3 A2;
        EXPECT_TRUE(A2 == BMat4x3({1.0, 0, 0, 0.0, 1.0, 0, 0.0, 0, 1.0, 0, 0, 0}));

        BMat4x3 A3 = A2;
        EXPECT_TRUE(A3 == A2);
    }

    // 测试赋值
    {
        BMat3x4 A({1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        BMat3x4 A2;
        A2 = A;
        EXPECT_TRUE(A2 == A);
    }

    // 测试 ()
    {
        double arr[] = {1.2, 2.5, 34, 4.8, 5.1, 6.9, 7.1, 8.2, 9.7, 10.5, 11.4, 12.2};

        // 测试取引用
        BMat3x4 A({1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 4; j++)
                A(i, j) = arr[i * 4 + j];
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 4; j++)
                EXPECT_FLOAT_EQ(A(i, j), arr[i * 4 + j]);
    }

    // 测试四则运算
    {
        BMat3x4 A({1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        BMat3x4 A2 = A * 2;
        EXPECT_TRUE(A2 == BMat3x4({2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24}));

        BMat3x4 A3 = A2 / 2;
        EXPECT_TRUE(A3 == A);

        BMat3x4 A4 = A + A;
        EXPECT_TRUE(A4 == A2);

        BMat3x4 A5 = A2 - A;
        EXPECT_TRUE(A5 == A);

        BVecN<4> v({1, 2, 3, 4});
        auto v2 = A * v;
        auto v3 = BVecN<3>({30.0, 70.0, 110.0});
        EXPECT_TRUE(v2 == v3);

        BMat4x3 B({1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        BMat3x3 A6 = A * B;
        EXPECT_TRUE(A6 == BMat3x3({70, 80, 90, 158, 184, 210, 246, 288, 330}));

        BMat3x4 A7 = -A;
        EXPECT_TRUE((A7 + A) == BMat3x4({0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}));
    }

    // 转置
    {
        BMat4x4 A({1, 4, 7, 0, 2, 5, 8, 0, 3, 6, 10, 0, 0, 0, 0, 1});
        BMat4x4 B = Transform::Transposed(A);
        EXPECT_TRUE(B == BMat4x4({1, 2, 3, 0, 4, 5, 6, 0, 7, 8, 10, 0, 0, 0, 0, 1}));
    }

    // 求逆
    {
        BMat4x4 A({1, 4, 7, 0, 2, 5, 8, 0, 3, 6, 10, 0, 0, 0, 0, 1});
        BMat4x4 B = Transform::Inverted(A);
        EXPECT_TRUE(B == BMat4x4({-2.0 / 3, -2.0 / 3, 1.0, 0.0, -4.0 / 3, 11.0 / 3, -2.0, 0.0, 1.0, -2.0, 1.0, 0.0, 0.0,
                                  0.0, 0.0, 1.0}));
    }

    // 正交投影
    {
        auto P = Transform::Ortho(-8, 9, -3, 2, 5, 100);
        BMat4x4 Q({
            0.117647,
            0,
            0,
            -0.0588235,
            0,
            0.4,
            0,
            0.2,
            0,
            0,
            -0.0210526,
            -1.10526,
            0,
            0,
            0,
            1,
        });

        EXPECT_TRUE(P == Q);
    }

    // 透视变换
    {
        auto P = Transform::Perspective(65, 1.8, 0.7, 250);
        BMat4x4 Q({
            0.872048,
            0,
            0,
            0,
            0,
            1.56969,
            0,
            0,
            0,
            0,
            -1.00562,
            -1.40393,
            0,
            0,
            -1,
            0,
        });

        EXPECT_TRUE(P == Q);
    }

    // lookAt
    {
        auto P = Transform::LookAt({1, 2, 2}, {4, 5, 6}, {7, 8, 9});

        BMat4x4 Q({-0.845154, 0.169031, 0.507093, -0.507093, -0.144943, 0.840668, -0.521794, -0.492805, -0.514496,
                   -0.514496, -0.685994, 2.91548, 0, 0, 0, 1});

        EXPECT_TRUE(P == Q);
    }
}

// 测试 RBT
TEST(TestRBT, BasisFunctions)
{
    int n = 200;
    std::vector<int> x(n, 0);
    for (int i = 0; i < n; i++)
    {
        x[i] = rand() % 1000;
        int sign = rand() % 2 == 0 ? 1 : -1;
        x[i] *= sign;
    }

    RBT<int> rbt;
    for (int i = 0; i < n; i++)
        rbt.Insert(x[i]);

    std::sort(x.begin(), x.end());
    while (!x.empty())
    {
        EXPECT_TRUE(rbt.BlackHeight() < (rbt.Height() * 2));
        EXPECT_EQ(x.size(), rbt.Size());
        EXPECT_EQ(x.front(), rbt.Min()->value);
        EXPECT_EQ(x.back(), rbt.Max()->value);
        // if (rbt.Size() < 40)
        //     rbt.Draw();

        for (int j = 0; j < x.size(); j++)
        {
            // 注意这里如果有重复元素，IndexOf 获得的是第一次出现的位置，因此不比较索引而是直接比较值
            EXPECT_DOUBLE_EQ(x[j], rbt[j]);
            EXPECT_EQ(x[rbt.IndexOf(x[j])], x[j]);
        }

        int d = rand() % x.size();
        auto it = x.begin() + d;
        rbt.Remove(*it);
        x.erase(it);
    }
}

// 测试 Heap
TEST(TestHeap, BasisFunctions)
{
    {
        Heap<double> heap = {5, 4, 8, 3, 5, 6, 7, 9};
        heap.Sort();
        EXPECT_FLOAT_EQ(heap.Front(), 3);
    }
    {
        Heap<double, std::less<double>> heap = {5, 4, 8, 3, 5, 6, 7, 9};
        heap.Sort();
        EXPECT_FLOAT_EQ(heap.Front(), 9);
    }
}